#include <mml/polygon.h>
#include <mml/primitives.h>



/** Area and centroid of concave polygon. */
void mml::analysePolygon(int n,
                         double const* vertexData,
                         double normal[3],
                         double centroid[3],
                         double& area,
                         double& projectedArea)
{
  // Set normal, centroid and area to zero:
  double projectionNormal[3];
  vector_reset(3, projectionNormal);
  vector_reset(3, normal);
  vector_reset(3, centroid);
  area = 0.0;
  projectedArea = 0.0;

  // \sum{c_i n_i^T}:
  double ciniTotal[9];
  matrix_reset(3, 3, ciniTotal);

  // Determine local normals and total normal:
  double onethird = 1.0 / 3.0;
  double oneNth   = 1.0 / n;
  double const* p0 = &vertexData[3 * (n - 1)];  // last point
  double q0[3] = { p0[0], p0[1], 0.0 }; // projected last point
  for (int i = 0; i < n - 2; ++i)
  {
    double const* p1 = &vertexData[3 * i];
    double const* p2 = &vertexData[3 * i + 3];
    double q1[3] = { p1[0], p1[1], 0.0 }; // projected ..
    double q2[3] = { p2[0], p2[1], 0.0 }; // .. vertices

    // p1 - p0
    double diff1[3];
    vector_copy(3, diff1, p1);
    vector_subtract(3, diff1, p0);
    double qdiff1[3];
    vector_copy(3, qdiff1, q1);
    vector_subtract(3, qdiff1, q0);

    // p2 - p0
    double diff2[3];
    vector_copy(3, diff2, p2);
    vector_subtract(3, diff2, p0);
    double qdiff2[3];
    vector_copy(3, qdiff2, q2);
    vector_subtract(3, qdiff2, q0);

    // (p1 - p0) x (p2 - p0):
    double localNormal[3];
    vector_cross(diff1, diff2, localNormal);
    double projectionLocalNormal[3];
    vector_cross(qdiff1, qdiff2, projectionLocalNormal);

    // Add to total normal:
    vector_add(3, normal, localNormal);
    vector_add(3, projectionNormal, projectionLocalNormal);

    // Construct local centroid:
    double localCentroid[3];
    vector_copy(3, localCentroid, p1);
    vector_add(3, localCentroid, p2);

    // Accumulate outer product of local centroid and local normal:
    double cini[9];
    vector_outerProduct(3, 3, localCentroid, localNormal, cini);
    matrix_add(3, 3, ciniTotal, cini);
  }

  // Squared norm of normal:
  area = vector_normsq(3, normal);
  projectedArea = vector_normsq(3, projectionNormal);

  if (area > 0.0) {
    // Non-singular polygon.

    // Compute norm and norm reciprocal:
    area = sqrt(area);  // norm
    projectedArea = sqrt(projectedArea);
    double recNorm = 1.0 / area;  // norm reciprocal

    // Normalize normal and compute area:
    vector_scale(3, normal, recNorm);  // normalize
    area *= 0.5;  // area
    projectedArea *= 0.5;  // area

    // centroid = \sum{c_i n_i^T} * normal
    matrix_matvecmul(3, 3, ciniTotal, normal, centroid);

    // Scale by total weight:
    vector_scale(3, centroid, onethird * recNorm);

    // Mix in p0:
    vector_add(3, centroid, p0, onethird);
  }
  else if (n > 0) {
    for (int i = 0; i < n; ++i) {
      vector_add(3, centroid, &vertexData[3 * i]);
    }
    vector_scale(3, centroid, oneNth);
  }
}
